#include "balance.h"
#include "arm_math.h"
#include "math.h"
#include "user_lib.h"

float NormalforceSolve(LinkNPodParam *lp, LinkNPodParam *rp, INS_t *imu, float delta_t)
{
  static float l_theta_last, l_leg_last, l_theta_dd, l_leg_dd;
  static float r_theta_last, r_leg_last, r_theta_dd, r_leg_dd;

  l_theta_dd = (lp->theta_w - l_theta_last) / delta_t;
  l_leg_dd = (lp->legd - l_leg_last) / delta_t;
  l_theta_last = lp->theta_w;
  l_leg_last = lp->legd;
  r_theta_dd = (rp->theta_w - r_theta_last) / delta_t;
  r_leg_dd = (rp->legd - r_leg_last) / delta_t;
  r_theta_last = rp->theta_w;
  r_leg_last = rp->legd;

  static float l_zw_dd;
  l_zw_dd = imu->MotionAccel_b[Z] - l_leg_dd * mcos(lp->theta) + 2.0 * lp->legd * lp->theta_w * msin(lp->theta) + lp->leg_len * l_theta_dd * msin(lp->theta) + lp->leg_len * lp->theta_w * lp->theta_w * mcos(lp->theta);
  static float r_zw_dd;
  r_zw_dd = imu->MotionAccel_b[Z] - r_leg_dd * mcos(rp->theta) + 2.0 * rp->legd * rp->theta_w * msin(rp->theta) + rp->leg_len * r_theta_dd * msin(rp->theta) + rp->leg_len * rp->theta_w * rp->theta_w * mcos(rp->theta);

  lp->normal_force = WHEEL_WEIGHT * l_zw_dd + WHEEL_WEIGHT * Gravity_mss + lp->F_leg * mcos(lp->theta) + lp->T_hip * msin(lp->theta) / lp->leg_len;
  rp->normal_force = WHEEL_WEIGHT * r_zw_dd + WHEEL_WEIGHT * Gravity_mss + rp->F_leg * mcos(rp->theta) + rp->T_hip * msin(rp->theta) / rp->leg_len;
}
